#include "Tore.hpp"

#include "Algebre/Solveur.hpp"
#include "Algebre/ConstruitMatrice.hpp"

#include "Utils/Erreur.hpp"
#include "Utils/debugstreams.hpp"
#include "Utils/StatusCode.hpp"

#include <cstdlib>
#include <cmath>

#include <vector>

#include <iostream>

// pour debugger uniquement
//#include <fstream>

using namespace std;

/**
 * Constructeur initialisant tous les champs du Tore. Les grand et
 * petit rayons sont automatiquement échangés au besoin.
 *
 * Puisque la transformation qui passe des coordonnées locales aux
 * coordonnées globales n'est pas univoquement définie, on teste ici 3
 * possibilités. Si aucune ne fonctionne, on arrête tout.
 *
 * Le solveur utilisé est déclaré et initialisé ici.
 *
 * Lance une exception de type Erreur s'il est impossible
 * d'initialiser la Transformation.
 *
 * @param pt centre du Tore.
 * @param v axe de symétrie du Tore.
 * @param grand grand rayon du Tore.
 * @param petit petit rayon du Tore.
 */
Tore::Tore(const Coord& pt, const VecteurU& v, const double& grand,
	   const double& petit)
  : Forme( "Tore" ), Point( pt ), Axe( v ), R( max( grand, petit ) ),
    r( min( grand, petit ) )
{
  MoteurResolution = new Solveur();
  PseudoCentre = Point;
  PseudoRayon = R + r;

  /*
   * On initialise la transformation ici
   */
  MatriceSO3 test;

  StatusCode sc( axeZ( Axe, test ) );

  if ( ! sc )
    {
      error() << sc.message() << endreq;
      exit( 1 );
    }
  else
    {
      VersGlobal = Transformation( test, Point );
      VersLocal  = VersGlobal.inverse();
    }
}

/**
 * Le destructeur doit libérer la mémoire allouée pour le solveur.
 */
Tore::~Tore()
{}

/**
 * Constructeur de copie, permettant de dupliquer un Tore.
 *
 * @param tore objet à copier.
 */
Tore::Tore(const Tore& tore)
  : Forme( "Tore", tore.VersGlobal ), Point( tore.Point ), Axe( tore.Axe ),
    R( tore.R ), r( tore.r )
{
  //Transformation = tore.Transformation;
  MoteurResolution = new Solveur();
  PseudoCentre = Point;
  PseudoRayon = R + r;
}

/**
 * On passe en premier aux coordonnées locales, en faisant une
 * translation et en eppliquant la rotation définie par
 * VersGlobal.
 */
Coord Tore::trouveIntersection(Rayon const &ray) const
{
  VecteurU dir( ray.direction() );
  Coord origine( ray.origine() );

  // cout << "Coordonnées globales : dir = " << dir << ", origine = "
  //      << origine << endl;

  // On change de référentiel
  dir     = VersLocal( dir );
  origine = VersLocal( origine );

  // cout << "Coordonnées locales : dir = " << dir << ", origine = "
  //      << origine << endl;

  // On calcule les coefficients du polynôme du 4ème degré
  // a t^4 + b t^3 + c t^2 + d t + e
  double a( dir.norme() * dir.norme() ); // est 1 car c'est un vecteur unitaire
  double b( 4 * (dir * origine) );
  double c( 2 * ( origine.r2() + R * R - r * r ) +
	    4 * (dir * origine) * (dir * origine) -
	    4 * R * R * ( dir.x() * dir.x() + dir.y() * dir.y() ) );
  double d( 4 * (dir * origine) * ( origine.r2() + R * R - r * r ) -
	    8 * R * R * ( dir.x() * origine.x() + dir.y() * origine.y() ) );
  double e( ( origine.r2() + R * R - r * r ) * 
	    ( origine.r2() + R * R - r * r )  -
	    4 * R * R * ( origine.x() * origine.x() + 
			  origine.y() * origine.y() ) );

  // cout << "a = " << a << ", b = " << b << ", c = " << c << ", d = "
  //      << d << ", e = " << e << endl;

  MoteurResolution -> setParams( e, d, c, b, a );

  vector< double > solutions;

  MoteurResolution -> resoud( solutions );

  vector< double >::const_iterator It, Begin( solutions.begin() ),
    End( solutions.end() );

  // for (It = Begin; It != End; It++)
  //   cout << *It << " ";
  // cout << endl;
  for (It = Begin; It != End; It++)
    {
      if ( *It > 1e-7 )
	{
	  Vecteur local( origine );
	  local += (*It) * dir ;
	  // cout << "En coordonnées locales : " << local << endl;
	  local = VersGlobal( static_cast< Coord >( local ) );
	  // cout << "En coordonnées globales : " << local << endl;	  
	  return static_cast<Coord>( local );
	}
    }
  
  return NullePart;
}

/**
 * Ne pas oublier de passer en coordonnées locales pour savoir où le
 * point se trouve sur le Tore, puis repasser en coordonnées globales.
 */
VecteurU Tore::normale(Coord const &coord) const
{
  Coord pt( VersLocal( coord ) );

  Vecteur local( 4 * pt.x() * ( pt.r2() + R * R - r * r )
		 - 8 * R * R * pt.x(),
		 4 * pt.y() * ( pt.r2() + R * R - r * r )
		 - 8 * R * R * pt.y(),
		 4 * pt.z() * ( pt.r2() + R * R - r * r ) );

  return VecteurU( VersGlobal( local ) );
}

bool Tore::estDans(Coord const &coord) const
{
  Vecteur pt( VersLocal( coord ) );
  
  double dist( sqrt( pow( pt.x(), 2 ) + pow( pt.y(), 2 ) ) - R );

  if ( abs( dist ) > r + 1.e-7 )
    return false;

  if ( pow( pt.z(), 2 ) < pow( dist, 2 ) - pow( r, 2 ) )
    return true;
  else
    return false;
}

bool Tore::estSur(Coord const &coord) const
{
  Vecteur pt( VersLocal( coord ) );
  
  return ( abs( pow( pt * pt + R * R - r * r, 2 ) -
		4 * R * R * ( pt.x() * pt.x() + pt.y() * pt.y() ) ) < 1e-7 );
}

/*
 * Non implémenté ici.
 */
void Tore::mapping(const Coord &pt, int &i, int &j, const int &frequence,
		   const int &Hauteur, const int &Largeur) const
{
  Coord carte( intrinseque( pt ) );

  if ( carte == NullePart )
    {
      i = j = 0;
      return;
    }

  double phi( carte.x() / R ), theta( carte.y() / r );
  double t;

  t = double( Hauteur * frequence ) * theta / ( 2. * Forme::ValeurPi );
  j = static_cast< int >( floor( t ) );

  t = double( Largeur * frequence ) * phi / ( 2. * Forme::ValeurPi );
  i = static_cast< int >( floor( t ) );

  i = i % Largeur;
  j = j % Hauteur;

  //log << "i = " << i << ", j = " << j << endl;
  //log.close();
}


/**
 * Permet de récupérer l'axe de symétrie du Tore.
 *
 * @return une référence constante sur Axe.
 */
const VecteurU& Tore::axe() const
{
  return Axe;
}

/**
 * Permet de récupérer le centre du Tore.
 *
 * @return une référence constante sur Point.
 */
const Coord& Tore::point() const
{
  return Point;
}

/**
 * Permet de récupérer la valeur du grand rayon.
 *
 * @return une référence constante sur R.
 */
const double& Tore::grand() const
{
  return R;
}

/**
 * Permet de récupérer la valeur du petit rayon.
 *
 * @return une référence constante sur r.
 */
const double& Tore::petit() const
{
  return r;
}

/**
 * Affiche Point, Axe, R et r.
 */
ostream& Tore::affiche(ostream& os) const
{
  os << "Forme : " << nom() << "Paramètres :" << endl
     << "\tCentre  : " << Point << endl
     << "\tAxe     : " << Axe << endl
     << "\tRayon 1 : " << R << endl
     << "\tRayon 2 : " << r << endl;

  return os;
}

/**
 * Les coordonnées intrinsèques sont \f$\phi\f$ et
 * \f$\theta\f$. L'angle \f$\phi\f$ est définit dans le plan
 * perpendiculaire à Oz. Il varie dans \f$\left[ 0, 2\pi \right[\f$, 0
 * correspond à un rayon-vecteur parralèle à Ox. Le sens de rotation
 * est le sens trigonométrique. L'angle \f$\theta\f$ est définit dans
 * le plan normal au plan Oxy contenant le rayon-vecteur. 0 correspond
 * à un vecteur parralèle à Oz, \f$\tfrac{\pi}{2}\f$ à un vecteur
 * colinéaire au premier rayon vecteur.
 *
 * On renvoie \f$ ( R \, \phi, r \, \theta, 0 ) \f$.
 */
Coord Tore::intrinseque(const Coord& pt) const
{
  if ( ! estSur( pt ) )
    {
      return NullePart;
    }

  double phi, theta;

  Coord local( VersLocal( pt ) );

  VecteurU Ox( 1., 0., 0. ), dsPlan( local.x(), local.y(), 0. );

  double rayon( sqrt( pow( local.x(), 2 ) + pow( local.y(), 2 ) ) );

  /*
    if ( abs( local.z() ) < 1e-7  && abs( rayon - R - r ) < 1e-7 )
    theta = Forme::ValeurPi / 2.;
    else if ( abs( local.z() ) < 1e-7  && abs( rayon - R + r ) < 1e-7 )
    theta =  3. * Forme::ValeurPi / 2.;
    else
  */
  theta = acos( local.z() / r );

  if ( abs( theta ) < 1.e-7 )
    theta = 0.;

  if ( rayon - R < -1.e-7 )
    theta = 2. * Forme::ValeurPi - theta;

  /*
  if ( abs( local.x() ) < 1.e-7 && local.y() > 1.e-7 )
    phi = 0.;
  else if ( abs( local.x() ) < 1.e-7 && local.y() < -1.e-7 )
    phi = Forme::ValeurPi;
  else
    phi = acos( Ox * dsPlan );
  */
  phi = acos( Ox * dsPlan );

  if ( abs( phi ) < 1.e-7 )
    phi = 0.;

  if ( local.y() < -1.e-7 )
    phi = 2. * Forme::ValeurPi - phi;

  if ( R * phi < 0. )
    warning() << "phi négatif" << endl;

  if ( r * theta < 0. )
    warning() << "theta négatif" << endl;

  return Coord( R * phi, r * theta, 0. );
}

/**
 * Ici on doit faire attention au fait que la forme a une période de
 * \f$2 \pi\f$ pour les deux coordonnées : il faut donc s'arranger
 * pour que la parité soit bien respectée. On multiplie alors
 * simplement les deux coordonnées intrinsèques par
 * \f$\tfrac{\text{larg}}{\pi}\f$. On triche donc : la largeur donne
 * alors le nombre de carreaux.
 */
Coord Tore::intrinsequeReduite(const Coord& pt, const double& larg) const
{
  if ( ! estSur( pt ) )
    {
      return NullePart;
    }

  Coord carte( intrinseque( pt ) );

  return carte *= ( larg / Forme::ValeurPi );
}
